#! /bin/python3
# -*- coding=UTF-8 -*-

import json
import numpy as np
import time
import math
import argparse

'''
usage: checkAutoMode.py [-h] [-f POINTS_FILE] [-p PEARSON] [-g POS_GAP] [-l LOCATION_MAX] [-fp POS_FIXED]
                         [-fh HEADING_FIXED] [--version]

optional arguments:
  -h, --help         show this help message and exit
  -f POINTS_FILE     GPS经纬度和解状态存放文件，默认值:collect_gps_demo.json
  -p PEARSON         皮尔森（pearson）相关系数，大于该值的点可能被剔除，默认值:0.99
  -g POS_GAP         预测点和实际点的距离，小于该值的点可能被剔除，默认值:0.1
  -l LOCATION_MAX    定位增强最大距离，单位:米，默认值:20.
  -nb NEIGHBOUR_MIN  相邻点的最小距离，单位:米，默认值:0.01
  -pf POS_FIXED      定位固定解状态值，默认值:4
  -hf HEADING_FIXED  朝向固定解状态值，默认值:50
  --version          show program's version number and exit
  
  脚本退出码为EXIT_AUTOMODE则可自动驾驶，EXIT_NO_AUTOMODE为不可自动驾驶
'''

configs = None

#可自动驾驶退出码
EXIT_AUTOMODE = 2000 
#不可自动驾驶退出码
EXIT_NO_AUTOMODE = 2001

def distance(point):
     return math.sqrt(math.pow(point[0],2)+math.pow(point[1],2))
 
def runWithAutoMode(points):
    len_all = len(points)
    if len_all < 3:
        print("点太少")
        return False
    L = 0
    for j in range(len_all - 1):
        # 计算（j,j+1）距离
        l_jj1 = distance(points[j+1] - points[j])
        if l_jj1 <= configs.location_max:
            # 判断是否固定解
            if points[j][2] == configs.pos_fixed and points[j][3] == configs.heading_fixed:
                # 固定解
                L = 0
                continue
            else:
                # 非固定解
                L += l_jj1
                if L > configs.location_max:
                    # 无固定解距离大于定位增强距离，结束遍历
                    print('无固定解距离大于定位增强距离，结束遍历',j)
                    break
        else:
            # 超过定位增强距离，结束
            L = l_jj1
            print('超过定位增强距离，结束',j,L)
            break
    
    if L > configs.location_max:
        return False
    else:
        return True
            
    
def PointOptimization(origin_list):
    geofence_point_list = []
    len_old = len(origin_list)
    ############进行点的优化处理#############
    
    geofence_transed = np.array(origin_list)
    
    '''
    #################### 构造数据
    b = np.ones((len_old,1))*configs.pos_fixed
    pointsww = np.c_[geofence_transed,b]
    b = np.ones((len_old,1))*configs.heading_fixed
    pointsww = np.c_[pointsww,b]
    #################### 构造数据
    
    with open("output_optimized2.json","w") as dump_f:
        json.dump(pointsww.tolist(),dump_f)
    '''       

    (row,col) = geofence_transed.shape

    if col != 4:
        print("数据格式不对,应当包含4列数据，分别为x,y,定位解状态,朝向解状态")
        print(geofence_transed.shape)
        exit(EXIT_NO_AUTOMODE)
    
    x_min = min(geofence_transed[:,0])    #取x轴坐标最小值
    y_min = min(geofence_transed[:,1])    #取y轴坐标最小值
    
    # Step 1: 坐标转换
    geofence_transed = geofence_transed-[x_min,y_min,0,0]
    
    # 循环次数
    last_cycle = i = 0

    while True:
        
        (long,lat) = np.shape(geofence_transed) #long是点数 lat是2

        can_be_delete = []      # 待删除点

        # Step 2: 用3的余数，来找到被删除点，在三个点中的顺序
        start_of_points = i%3

        # Step 3: 进入一次筛选过程
        j = start_of_points
        
        while j < long:
            if j+3 > long: # 剩余点数不足3个，退出
                break
            else:
                # Step 3.1: 取3个连续点
                simplify_points = geofence_transed[j:j+3]

                # 删除过近的相邻点
                d12 = distance(simplify_points[1]-simplify_points[0])
                if d12 < configs.neighbour_min:
                    can_be_delete.append(j+1)
                    j+=2
                    continue

#               # Step 3.2:线性拟合
                # Step 3.2.1: 构造拟合函数
                linear_fitted = np.polyfit([simplify_points[0][0],simplify_points[2][0]],[simplify_points[0][1],simplify_points[2][1]],deg=1)
                # Step 3.2.2: 计算预测点linear_points[0] [1] [2]
                linear_points = linear_fitted[0]*simplify_points[:,0] +linear_fitted[1]
            
                # Step 3.2.3: 计算预测点与绝对点的差值绝对值
                abs_pos_gap = abs(linear_points[1] - simplify_points[1][1])
                
                d13 = distance(simplify_points[2]-simplify_points[0])
                # Step 3.2.4: corrcoef()函数求实际点与预测的linear_points之间的相关系数
                pearson = np.corrcoef(simplify_points[:,1],linear_points)[0][1]

                # Step 3.2.5: 标记可以被删除的点
                # judgement one: pearson 相关系数
                # judgement two: absolute position gap 绝对定位差值
                if pearson > configs.pearson and abs_pos_gap < configs.pos_gap and d13 < configs.location_max:
                    # 三个点的下下标分别为j,j+1,j+2，标记待删除中点
                    can_be_delete.append(j+1)
                
                # Step 3.2.6: 下标后移2,以第三个点为起点，继续筛选
                j += 2

        # Step 4: 删除标记点
        if(len(can_be_delete) == 0):
            if (last_cycle + 1 == i): # 连续两次待删除点为空退出优化流程
                len_new = len(geofence_transed)
                print ("Point optimize completed: ", i, len_new, len_old, '{:.2%}'.format(len_new/len_old))
                autoMode = runWithAutoMode(geofence_transed)
                if autoMode:
                    exit(EXIT_AUTOMODE) #能自动
                else:
                    exit(EXIT_NO_AUTOMODE) #不能自动
                break;
            else:
                last_cycle = i
        else:
            can_be_delete.sort(reverse=True)#降序
            for to_delete_ii in can_be_delete:
                geofence_transed = np.delete(geofence_transed,to_delete_ii,axis=0)
        # Step 5: 进入下一轮遍历
        i += 1

    # Step 6: 坐标转换
    # geofence_transed = geofence_transed + [x_min,y_min,0,0]
    # Step 7: array转回list
    # geofence_point_list = geofence_transed.tolist()

    # return geofence_point_list

def GeofenceDictCreate():
    with open(configs.points_file,'r') as load_f:
        load_dict = json.load(load_f)
        l=[]
        for _, val in enumerate(load_dict):
            tmplist=[]
            tmplist.append(val["positionX"])
            tmplist.append(val["positionY"])
            tmplist.append(val["locSlnstate"])
            tmplist.append(val["aglSlnstate"])
            l.append(tmplist)
        PointOptimization(l)

def JSONCreate():
    starttime = time.time()
    
    GeofenceDictCreate()

    endtime = time.time()
    dtime = endtime - starttime
    print("time cost:%s s" % dtime)


def paser_init():
    parser = argparse.ArgumentParser()
    
    parser.add_argument('-f', action='store', dest='points_file', default = 'collect_gps_demo.json',
        help='GPS经纬度和解状态存放文件，默认值:collect_gps_demo.json')
    parser.add_argument('-p', action='store', dest='pearson', default = 0.99,
        help='皮尔森（pearson）相关系数，大于该值的点可能被剔除，默认值:0.99')
    parser.add_argument('-g', action='store', dest='pos_gap', default=0.1,
        help='预测点和实际点的距离，小于该值的点可能被剔除，默认值:0.1')
    parser.add_argument('-l', action='store', dest='location_max', default=20.,
        help='定位增强最大距离，单位:米，默认值:20.')
    parser.add_argument('-nb', action='store', dest='neighbour_min', default=0.01,
        help='相邻点的最小距离，单位:米，默认值:0.01')
    parser.add_argument('-pf', action='store', dest='pos_fixed', default=4,
        help='定位固定解状态值，默认值:4')
    parser.add_argument('-hf', action='store', dest='heading_fixed', default=50,
        help='朝向固定解状态值，默认值:50')
    
    parser.add_argument('--version', action='version', version='%(prog)s 1.0')
    
    return parser

def showConfig():
    print ('points_file     = ', configs.points_file)
    print ('pearson         = ', configs.pearson)
    print ('pos_gap         = ', configs.pos_gap)
    print ('location_max    = ', configs.location_max)
    print ('neighbour_min   = ', configs.neighbour_min)
    print ('pos_fixed       = ', configs.pos_fixed)
    print ('heading_fixed   = ', configs.heading_fixed)


def main():
    parser = paser_init()
    global configs
    configs = parser.parse_args()
    showConfig()
    JSONCreate()

if __name__ == "__main__":
    main()
